#include "map_point_smooth_processor.h"

namespace vehicle {
namespace map {

void MapPointSmoothProcess::MapPointSmoothInit(const std::vector<STRoadPoint> &org_point)
{
    x_.clear();
    y_.clear();
    heading_.clear();
    curvature_.clear();
    s_.clear();
    if(org_point.size()>0)
    {
        for(int kk=0;kk<org_point.size();kk++)
        {
            x_.emplace_back(org_point.at(kk).global_x);
            y_.emplace_back(org_point.at(kk).global_y);
        }
        s_.emplace_back(0.);
        double dx = x_[1]-x_[0];
        double dy = y_[1]-y_[0];
        s_.push_back(0);
        heading_.emplace_back(std::atan2(dy, dx));
        for(int kk =1;kk<x_.size();kk++)
        {
            double dx = x_[kk] - x_[kk-1];
            double dy = y_[kk] - y_[kk-1];
            if(kk == 1)
            {
                heading_.emplace_back(std::atan2(dy, dx));
            }
            s_.emplace_back(s_.back()+std::hypot(dy, dx));
            heading_.emplace_back(std::atan2(dy, dx));
        }

//        for(int kk =1;kk<x_.size();kk++)
//        {
//            printf("%d %f\r\n",kk,heading_[kk]);
//        }

//        printf("------------------------------------\r\n");
        received_ = true;
    }
}

float MapPointSmoothProcess::getS(int index)
{
     return s_[index];
}

bool MapPointSmoothProcess::LoadLocalizeInSegmentMap(STRoadPoint current_state, std::vector<STRoadPoint> &segment_points, int &near_index,float km_dis,float kp_dis)
{
    double ego_x = current_state.global_x;
    double ego_y = current_state.global_y;
    double ego_heading = current_state.theta;
    int min_index;
    double min_distance = std::numeric_limits<double>::infinity();
    if(x_.size() == 0 || y_.size() == 0)
    {
        return false;
    }
    for(int j=0;j<x_.size();j++)
    {
        double heading_error = heading_[j] - ego_heading;
        heading_error = vehicle::tools::cast_from_PI_to_PI_Angle(heading_error);
        if(std::fabs(heading_error) > M_PI/2)
        {
            continue;
        }
        double distance = hypot(x_[j] - ego_x, y_[j] - ego_y);
        if(distance < min_distance)
        {
            min_distance = distance;
            min_index = static_cast<int>(j);
        }
    }
#if 1
    near_index = min_index;
    int idx_kp1 = std::min<int>(min_index + 1, static_cast<int>(x_.size()) - 1);   //后面的点
    int idx_km1 = std::max<int>(min_index - 1, 0);//前面的点

    double heading_vec[2] = {
        x_[idx_kp1] - x_[idx_km1],  //车道上最近点的朝向
        y_[idx_kp1] - y_[idx_km1],
    };

    double ego_vec[2] = {//现在位置当前点的朝向
        ego_x - x_[idx_km1],
        ego_y - y_[idx_km1],
    };
    double projection_s =  //将最近点单位化，再将投影
        (ego_vec[0] * heading_vec[0] + ego_vec[1] * heading_vec[1]) /
        std::hypot(heading_vec[0], heading_vec[1]);
//    printf("^^^^^^^^^^^^^^%f\r\n",projection_s);
    projection_s += s_[idx_km1];   //求最近的点到车道段起点的距离

//    printf("------------------%f\r\n",projection_s);

    printf("idx_kp1 %d,idx_km1 %d\r\n",idx_kp1,idx_km1);
    double routing_segment_heading = atan2(heading_vec[1], heading_vec[0]); //全局坐标的朝向
    double heading_error = routing_segment_heading - ego_heading;
    if (std::fabs(heading_error) > M_PI) {
      heading_error = atan2(sin(heading_error), cos(heading_error));   //转为（-pi - pi）
    }
    if(projection_s < s_.front())
        projection_s = s_.front();
    else if(projection_s > s_.back())
        projection_s = s_.back();
    if (std::fabs(heading_error) < M_PI / 2) {
      if(!LoadSmoothRawData(projection_s, idx_kp1, idx_km1, s_, x_,
                             y_, segment_points,km_dis,kp_dis))
          return false;
    }else{
        printf("heading_error is %lf, heading error > M_PI /2 \r\n",heading_error);
        return false;

    }

//   printf("km %d,kp %d,size %d, s %f\r\n",idx_km1,idx_kp1,segment_points.size(),projection_s);
#endif
    return true;
}


bool MapPointSmoothProcess::LoadSmoothRawData(const double& projection_s, int idx_kp1,int idx_km1, const std::vector<double>& s,const std::vector<double>& x,const std::vector<double>& y,std::vector<STRoadPoint>& segment_points,float km_dis,float kp_dis)
{
//    double local_spline_half_range = 25;
    double local_spline_half_range_km = km_dis;
    double local_spline_half_range_kp = kp_dis;
    double interpolation_resolution = 1;

    if (projection_s < s.front() || projection_s > s.back()) {
      return false;
    }

    while (idx_km1 >= 0) {
      if (projection_s - s[idx_km1] > local_spline_half_range_km) {
        break;
      }
      idx_km1--;
    }
    idx_km1 = std::max<int>(idx_km1, 0);
    while (idx_kp1 < x.size()) {
      if (s[idx_kp1] - projection_s > local_spline_half_range_kp) {
        break;
      }
      idx_kp1++;
    }
    idx_kp1 = std::min<int>(idx_kp1, x.size() - 1);

//    if (projection_s - x[idx_km1] < local_spline_half_range_km) {
//      while (idx_kp1 < x.size()) {
//          if (s[idx_kp1] - s[idx_km1] > 2 * local_spline_half_range) {
//            break;
//          }
//          idx_kp1++;
//        }
//        idx_kp1 = std::min<int>(idx_kp1, x.size() - 1);
//      }

//      if (s[idx_kp1] - projection_s < local_spline_half_range_kp) {
//        while (idx_km1 >= 0) {
//          if (s[idx_kp1] - s[idx_km1] > 2 * local_spline_half_range) {
//            break;
//          }
//          idx_km1--;
//        }
//        idx_km1 = std::max<int>(idx_km1, 0);
//      }
      // Interpolate to get raw points.
      std::vector<double> local_s;
      std::vector<double> local_x;
      std::vector<double> local_y;

      local_x.resize(idx_kp1 - idx_km1 + 1);
      local_y.resize(idx_kp1 - idx_km1 + 1);
      local_s.resize(idx_kp1 - idx_km1 + 1);
      for (size_t i = 0; i < idx_kp1 - idx_km1 + 1; ++i) {
        local_x[i] = x[idx_km1 + i];   //最近的点相应前面的点和后面的点 坐标
        local_y[i] = y[idx_km1 + i];
        local_s[i] = s[idx_km1 + i];
      }
      vehicle::common::math::LinearSpline sx;
      vehicle::common::math::LinearSpline sy;
      sx.Build(local_s, local_x);
      sy.Build(local_s, local_y);
      std::vector<double> raw_s;
      raw_s.clear();
      for(float i=local_s.front();i<local_s.back();)
      {
          raw_s.push_back(i);
          i =  i + interpolation_resolution;
      }
      segment_points.clear();
      STRoadPoint point;
      for (const auto s : raw_s) {
          point.global_x = sx(s);
          point.global_y = sy(s);
          segment_points.push_back(point);
      }
      return true;
}

}
}
